/*
 * usb_l.cpp
 *
 *  Created on: Jul 5, 2013
 *      Author: g.kruglov
 */

#include "usb_l.h"
#include "kl_lib_L15x.h"

//#define EP0_VERBOSE

#ifdef EP0_VERBOSE
#define EP0_PRINT(S)                Uart.Printf(S)
#define EP0_PRINT_V1(S, a1)         Uart.Printf(S, a1)
#define EP0_PRINT_V1V2(S, a1, a2)   Uart.Printf(S, a1, a2)
#else
#define EP0_PRINT(S)
#define EP0_PRINT_V1(S, a1)
#define EP0_PRINT_V1V2(S, a1, a2)
#endif

// USB BufTable registers block
struct BTableReg_t {
  volatile uint32_t TxAddr;     // TX buffer offset register
  volatile uint16_t TxCnt0;     // TX counter register 0
  volatile uint16_t TxCnt1;     // TX counter register 1
  volatile uint32_t RxAddr;     // RX buffer offset register
  volatile uint16_t RxCnt0;     // RX counter register 0
  volatile uint16_t RxCnt1;     // RX counter register 1
};
#define GET_BTB_REG(id)     ((BTableReg_t*)((uint32_t)STM32_USBRAM_BASE + \
                            (uint32_t)STM32_USB->BTABLE * 2 + \
                            (uint32_t)(id) * sizeof(BTableReg_t)))
#define BTB_ADDR2PTR(addr)  ((uint32_t *)((addr) * 2 + STM32_USBRAM_BASE))

static uint8_t ZeroArr[2] = {0, 0};
Usb_t Usb;

void Usb_t::Init() {
    Disconnect();
    // GPIO does not require setup
    // Clock
    rccEnableUSB(FALSE);
    // Switch on analog part, holding usb in reset state
    STM32_USB->CNTR = CNTR_FRES;    // Fres=1, Pwdn=0
    // Enable irqs
    nvicEnableVector(STM32_USB1_LP_NUMBER, CORTEX_PRIORITY_MASK(STM32_USB_USB1_LP_IRQ_PRIORITY));
    // Remove reset
    STM32_USB->CNTR = 0;
    IReset();
}

void Usb_t::IReset() {
    State = usConnected;
    PktMemReset();
    STM32_USB->BTABLE = 0;          // Clear table address
    STM32_USB->ISTR = 0;            // Clear interrupts
    STM32_USB->DADDR = DADDR_EF;    // Enable USB device with zero Addr
    STM32_USB->CNTR =   CNTR_RESETM |
                        CNTR_SUSPM  |
                        CNTR_WKUPM  |
                        CNTR_CTRM;  // Enable Reset and CorrectTransfer irqs
    for(uint8_t i=0; i<EP_CNT; i++) Ep[i].Init(&EpCfg[i]);
    Ep[0].State = esSetup;
}


void Usb_t::PktMemReset() {
    PktMemNext = 4 * sizeof(BTableReg_t);
}
uint16_t Usb_t::PktMemAlloc(uint16_t Sz) {
    uint16_t ret = PktMemNext;
    PktMemNext += Sz;
    return ret;
}


// ========================= Data operations ===================================
void Usb_t::StartTransmitBuf(uint8_t EpID, Buf_t ABuf) {
    chSysLock();
    Ep[EpID].State = esDataIn;
    Ep[EpID].BufIn = ABuf;
    Ep[EpID].TransmitDataChunk();
    chSysUnlock();
}

uint8_t Usb_t::WaitTransactionEnd(uint8_t EpID) {
    uint8_t rslt = OK;
    chSysLock();
    if(Ep[EpID].State != esIdle) {  // Transaction may end before call of this function
        Ep[EpID].PThread = chThdSelf();
        chSchGoSleepS(THD_STATE_SUSPENDED);
        if(chThdSelf()->p_u.rdymsg != RDY_OK) rslt = FAILURE;
    }
    chSysUnlock();
    return rslt;
}

// =============================== Ep_t ========================================
void Ep_t::Init(const EpCfg_t *PCfg) {
    Indx = PCfg->Indx;
    // Process waiting thread if any
    ResumeWaitingThd(RDY_RESET);
    // Set EP type, NAK both directions
    uint16_t epr;
    epr = PCfg->Type;
    epr |= EPR_STAT_TX_NAK;
    epr |= EPR_STAT_RX_NAK;
    // EPxR register setup
    EPR_SET(Indx, epr | Indx);
    EPR_TOGGLE(Indx, epr);
    // ==== Endpoint size and address initialization ====
    uint16_t BlSz, NBlocks;
    if(PCfg->OutMaxsize <= 62) {
        BlSz = 0;
        NBlocks = PCfg->OutMaxsize / 2;
    }
    else {
        BlSz = 0x8000;
        NBlocks = (PCfg->OutMaxsize / 32) - 1;
    }
    BTableReg_t *PReg = GET_BTB_REG(Indx);
    PReg->TxCnt0 = 0;
    PReg->RxCnt0 = BlSz | (NBlocks << 10);
    PReg->TxAddr = Usb.PktMemAlloc(PCfg->InMaxsize);
    PReg->RxAddr = Usb.PktMemAlloc(PCfg->OutMaxsize);
}

void Ep_t::ResumeWaitingThd(msg_t ReadyMsg) {
    if(PThread != NULL) {
        chSysLockFromIsr();
        if(PThread->p_state == THD_STATE_SUSPENDED) {
            PThread->p_u.rdymsg = ReadyMsg;
            chSchReadyI(PThread);
        }
        chSysUnlockFromIsr();
        PThread = NULL;
    }
}

uint16_t Ep_t::GetRxDataLength() {
    BTableReg_t *PReg = GET_BTB_REG(Indx);
    uint16_t Len = PReg->RxCnt0 & RXCOUNT_COUNT_MASK;
//    Uart.Printf("UL=%u\r", Len);
    return Len;
}

void Ep_t::ReadToBuf(uint8_t *PDstBuf, uint16_t Len) {
    BTableReg_t *PReg = GET_BTB_REG(Indx);
    uint32_t *PSrc = BTB_ADDR2PTR(PReg->RxAddr);
    Len = (Len + 1) / 2;
    for(uint8_t i=0; i < Len; i++) {
        *(uint16_t*)PDstBuf = (uint16_t)*PSrc++;
        PDstBuf += 2;
    }
}

void Ep_t::FlushRx(uint16_t Len) {
    BTableReg_t *PReg = GET_BTB_REG(Indx);
    uint32_t *PSrc = BTB_ADDR2PTR(PReg->RxAddr);
    Len = (Len + 1) / 2;
    for(uint8_t i=0; i < Len; i++) (void)*PSrc++;
}

void Ep_t::ReadToQueue(uint16_t Len) {
//    Uart.Printf("R2Q %u\r", Len);
    uint16_t w;
    BTableReg_t *PReg = GET_BTB_REG(Indx);
    uint32_t *PSrc = BTB_ADDR2PTR(PReg->RxAddr);
    bool OddLength = Len & 0x01;
    Len = Len / 2;
    chSysLockFromIsr();
    while(Len--) {
        w = (uint16_t)*PSrc++;
        chIQPutI(POutQueue, (uint8_t)(w & 0xFF));
        chIQPutI(POutQueue, (uint8_t)(w >> 8));
    }
    // Last byte for odd lengths
    if(OddLength) chIQPutI(POutQueue, (uint8_t)(*PSrc & 0xFF));
    chSysUnlockFromIsr();
}

// Fill USB memory with BufIn's data
void Ep_t::FillInBuf() {
    // Count of bytes to write to USB memory
    uint16_t n = (BufIn.Len > EpCfg[Indx].InMaxsize)? EpCfg[Indx].InMaxsize : BufIn.Len;
    // If last data chunk size equals Ep size, additional zero pkt must be transmitted
    TransmitFinalZeroPkt = (n == EpCfg[Indx].InMaxsize);
    BufIn.Len -= n;
    BTableReg_t *PReg = GET_BTB_REG(Indx);
    PReg->TxCnt0 = n;
    uint32_t *p = USB_ADDR2PTR(PReg->TxAddr);
    n = (n + 1) / 2;
    while(n > 0) {
        *p++ = *(uint16_t*)BufIn.Ptr;
        BufIn.Ptr += 2;
        n--;
    }
}

void Ep_t::TransmitZeroPkt() {
    GET_BTB_REG(Indx)->TxCnt0 = 0;
    StartInTransaction();
}
void Ep_t::ReceiveZeroPkt() {
//    Uart.Printf("Rx Zero\r");
    StartOutTransaction();
}


// ============================= Setup etc. ====================================
void Usb_t::SetupPktHandler() {
    EP0_PRINT("Setup\r");
//    Uart.Printf("%A\r", EpCfg[0].PBufOut, 8);
    // Try to handle request
    Buf_t Reply;
    if((SetupReq.bmRequestType & 0x60) != 0) Ep[0].State = esError;   // Non-standard request
    else Ep[0].State = DefaultReqHandler(&Reply);
    // Prepare to next transaction
    switch(Ep[0].State) {
        case esDataIn:
            Ep[0].BufIn = Reply;
            Ep[0].TransmitDataChunk();
            break;
        case esStatusIn:
            Ep[0].TransmitZeroPkt();
            break;
        default:
            Ep[0].StallIn();
            Ep[0].StallOut();
            break;
    } // switch
}

// Ep0 callbacks
static void SetAddress() {
    uint32_t Addr = Usb.IGetSetupAddr();
//    Uart.Printf("SetAddr %u\r", Addr);
    STM32_USB->DADDR = Addr | DADDR_EF;
}
static void SetStateCofigured() {
    Uart.Printf("UsbConfigured\r");
    Usb.State = usConfigured;
}

EpState_t Usb_t::DefaultReqHandler(Buf_t *PBuf) {
    uint8_t Recipient = SetupReq.bmRequestType & USB_RTYPE_RECIPIENT_MASK;
    if(Recipient == USB_RTYPE_RECIPIENT_DEVICE) {
        //Uart.Printf("Dev\r\n");
        switch(SetupReq.bRequest) {
            case USB_REQ_GET_STATUS:    // Just return the current status word
                EP0_PRINT("GetStatus\r");
                PBuf->Ptr = ZeroArr;    // Remote wakeup = 0, selfpowered = 0
                PBuf->Len = 2;
                return esDataIn;
                break;
            case USB_REQ_SET_ADDRESS:
                EP0_PRINT("SetAddr\r");
                PBuf->Len = 0;
                Ep[0].cbEndTransaction = SetAddress;
                return esStatusIn;
                break;
            case USB_REQ_GET_DESCRIPTOR:
                EP0_PRINT_V1V2("GetDesc t=%u i=%u\r", SetupReq.Type, SetupReq.Indx);
                GetDescriptor(SetupReq.Type, SetupReq.Indx, &PBuf->Ptr, &PBuf->Len);
                // Trim descriptor if needed, as host can request part of descriptor.
                TRIM_VALUE(PBuf->Len, SetupReq.wLength);
//                Uart.Printf("DescLen=%u\r", PBuf->Len);
                if(PBuf->Len != 0) return esDataIn;
                break;
            case USB_REQ_GET_CONFIGURATION:
                EP0_PRINT("GetCnf\r");
//                *Ptr = &Configuration;
//                *PLen = 1;
//                return OK;
                break;
            case USB_REQ_SET_CONFIGURATION:
                EP0_PRINT_V1("SetCnf %u\r", SetupReq.wValue);
                PBuf->Len = 0;
                Ep[0].cbEndTransaction = SetStateCofigured;
                return esStatusIn;
                break;
            default: break;
        } // switch
    }
    else if(Recipient == USB_RTYPE_RECIPIENT_INTERFACE) {
        if(SetupReq.bRequest == USB_REQ_GET_STATUS) {
            EP0_PRINT("InterfGetSta\r");
//            *Ptr = (uint8_t*)ZeroStatus;
//            *PLen = 2;
//            return OK;
        }
    }
    else if(Recipient == USB_RTYPE_RECIPIENT_ENDPOINT) {
        EP0_PRINT("Ep\r");
        switch(SetupReq.bRequest) {
            case USB_REQ_SYNCH_FRAME:
//                *Ptr = (uint8_t*)ZeroStatus;
//                *PLen = 2;
//                return OK;
                break;
            case USB_REQ_GET_STATUS:
                break;
        }
    }
    return esError;
}

// ================================ IRQ ========================================
void Usb_t::IIrqHandler() {
    uint32_t istr = STM32_USB->ISTR;
//    Uart.Printf("i=%X\r", istr);
    if(istr & ISTR_RESET) {
//        Uart.Printf("Rst\r");
        IReset();
        STM32_USB->ISTR = ~ISTR_RESET;
    }
    if(istr & ISTR_SUSP) {
//        Uart.Printf("Sup\r");
        STM32_USB->ISTR = ~ISTR_SUSP;
    }
    if(istr & ISTR_WKUP) {
//        Uart.Printf("Wup\r");
        STM32_USB->ISTR = ~ISTR_WKUP;
    }
    while(istr & ISTR_CTR) {
//        Uart.Printf("Ctr\r");
        uint16_t EpID = istr & ISTR_EP_ID_MASK;
        uint16_t epr = STM32_USB->EPR[EpID];
        if(epr & EPR_CTR_TX) ICtrHandlerIN(EpID);
        if(epr & EPR_CTR_RX) ICtrHandlerOUT(EpID, epr);
        istr = STM32_USB->ISTR;
    }
    STM32_USB->ISTR = 0;
}

// IN transaction
void Usb_t::ICtrHandlerIN(uint16_t EpID) {
//    Uart.Printf("IN ep%u\r", EpID);
    EPR_CLEAR_CTR_TX(EpID);
    Ep_t *ep = &Ep[EpID];
    if(EpID == 0) {
        switch(ep->State) {
            case esDataIn:
                if((ep->BufIn.Len != 0) or ep->TransmitFinalZeroPkt) {
                    ep->TransmitDataChunk(); // Transfer not completed
                }
                else {  // Buf len == 0
                    ep->State = esStatusIn;
                    ep->ReceiveZeroPkt();
                }
                break;

            case esStatusIn:
                if(ep->cbEndTransaction != NULL) ep->cbEndTransaction();
                ep->cbEndTransaction = NULL;
                ep->State = esSetup;
                break;

            default:
                EP0_PRINT("e\r");
                ep->State = esError;
                ep->StallIn();
                ep->StallOut();
                break;
        } // switch
    } // if(EpID == 0)
    else {
        if((ep->BufIn.Len != 0) or ep->TransmitFinalZeroPkt) {
            ep->TransmitDataChunk(); // Transfer not completed
        }
        else {  // Buf len == 0
            ep->State = esIdle;
            ep->ResumeWaitingThd(RDY_OK);
        }
    }
}

// OUT transaction
/* Beware: if Rx Length is read before setup pkt read out, SETUP bit may reset sometimes.
 * Workaround: read EPR first.
 */
void Usb_t::ICtrHandlerOUT(uint16_t EpID, uint16_t Epr) {
    //Uart.Printf("OUT ep%u\r", EpID);
    EPR_CLEAR_CTR_RX(EpID);
    Ep_t *ep = &Ep[EpID];
    uint16_t Len = ep->GetRxDataLength();
    if(EpID == 0) {
        if(Epr & EPR_SETUP) {
            ep->State = esSetup; // In case of setup pkt, reset stage to Setup one
            ep->ReadToBuf(Ep0OutBuf, 8);
            SetupPktHandler();
        }
        else {
//            Uart.Printf("Len=%u\r", Len);
            if(Len>0) {
//                ep->ReadToBuf(Ep0OutBuf, Len);
                //Uart.Printf("%A\r", Ep0OutBuf, Len);
            }
//            if(Len > EpCfg[EpID].OutMaxsize) Len = EpCfg[EpID].OutMaxsize;    // FIXME
//            ep->ReadRxData(EpCfg[EpID].PBufOut, Len);
//            if(Len < EpCfg[EpID].OutMaxsize) {} // Transfer completed
//            else ep->StartOutTransaction();     // Transfer not completed
        }
    } // if(EpID == 0)
    else {
//        Uart.Printf("Len=%u\r", Len);
        if(ep->POutQueue != NULL) ep->ReadToQueue(Len);
        else ep->FlushRx(Len);
    }
}

extern "C" {
CH_IRQ_HANDLER(STM32_USB1_LP_HANDLER) {
    CH_IRQ_PROLOGUE();
    Usb.IIrqHandler();
    CH_IRQ_EPILOGUE();
}
} // extern C
